#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :pid_controller.py
@Description  :
@Time         :2024/07/04 23:10:07
@Author       :Lin Yuheng
@Version      :1.0
'''

class PIDController:
    # PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
	# val_dot in pid_calculate() will be ignored

    def __init__(self, dt_min=0.001):
        # 时间参数
        self._dt = 0
        self._dt_default = 0.2
        self._dt_max = 1
        self._dt_min = dt_min

        # 积分微分蚕数
        self.input = 0
        self.integral = 0
        self.integral_limit = 0
        self.output = 0
        self.output_limit = 0
        self.last_output = 0
        self.error_previous = 0

        # pid参数
        self.Kp = 0
        self.Ki = 0
        self.Kd = 0

    def reset(self):
        self.input = 0
        self.integral = 0
        self.integral_limit = 0
        self.output_limit = 0
        self.last_output = 0
        self.error_previous = 0

    def set_pid_params(self, Kp, Ki, Kd, output_limit):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.output_limit = output_limit

    def pid_calculate(self, setpoint, val, dt):
        
        # 当前误差
        error = setpoint - val

        # 当前误差微分
        d = (error - self.error_previous) / max(dt, self._dt_min)
        self.error_previous = error

        # PD输出
        output = error * self.Kp + d * self.Kd

        # 限制输出
        if output > self.output_limit:
            output = self.output_limit
        elif output < -self.output_limit:
            output = -self.output_limit

        self.last_output = output

    def set_prev_output(self):
        self.last_output = self.output

    def get_output(self):
        return self.last_output

